// coord_system.cpp
#include "CoordSys.h"

EllipsoidParams::EllipsoidParams(CoordinateSystemType type) {
    if (type == CoordinateSystemType::BDCS) {
        semiMajor = 6378137.0;
        flattening = 1.0 / 298.257222101;
        eccSquared = 0.00669438002290;
    }
    else {
        semiMajor = 6378137.0;
        flattening = 1.0 / 298.257223563;
        eccSquared = 0.00669437999014;
    }
}

CoordinateConverter::CoordinateConverter(CoordinateSystemType type)
    : ellipsoid(type) {
}

CartesianPoint CoordinateConverter::geodeticToCartesian(const GeodeticCoord& geo) {
    double N = ellipsoid.getSemiMajor() /
        std::sqrt(1.0 - ellipsoid.getEccSquared() * std::sin(geo.latitude) * std::sin(geo.latitude));
    double factor = N + geo.height;
    double cosLat = std::cos(geo.latitude);

    return CartesianPoint(
        factor * cosLat * std::cos(geo.longitude),
        factor * cosLat * std::sin(geo.longitude),
        (N * (1.0 - ellipsoid.getEccSquared()) + geo.height) * std::sin(geo.latitude)
    );
}

GeodeticCoord CoordinateConverter::cartesianToGeodetic(const CartesianPoint& cart) {
    double p = std::sqrt(cart.x * cart.x + cart.y * cart.y);
    double theta = std::atan2(cart.z * ellipsoid.getSemiMajor(),
        p * std::sqrt(ellipsoid.getSemiMajor() * ellipsoid.getSemiMajor() -
            ellipsoid.getEccSquared() * ellipsoid.getSemiMajor() * ellipsoid.getSemiMajor()));

    GeodeticCoord geo;
    geo.longitude = std::atan2(cart.y, cart.x);

    double sinTheta = std::sin(theta);
    double cosTheta = std::cos(theta);
    double numerator = cart.z + (ellipsoid.getEccSquared() * ellipsoid.getSemiMajor() / (1.0 - ellipsoid.getEccSquared())) * sinTheta * sinTheta * sinTheta;
    double denominator = p - ellipsoid.getEccSquared() * ellipsoid.getSemiMajor() * cosTheta * cosTheta * cosTheta;
    geo.latitude = std::atan2(numerator, denominator);

    double N = ellipsoid.getSemiMajor() /
        std::sqrt(1.0 - ellipsoid.getEccSquared() * std::sin(geo.latitude) * std::sin(geo.latitude));
    geo.height = p / std::cos(geo.latitude) - N;

    return geo;
}

LocalENU CoordinateConverter::cartesianToLocalENU(const CartesianPoint& cart, const CartesianPoint& ref) {
    GeodeticCoord refGeo = cartesianToGeodetic(ref);
    CartesianPoint delta(cart.x - ref.x, cart.y - ref.y, cart.z - ref.z);

    double sinLat = std::sin(refGeo.latitude);
    double cosLat = std::cos(refGeo.latitude);
    double sinLon = std::sin(refGeo.longitude);
    double cosLon = std::cos(refGeo.longitude);

    return LocalENU{
        -sinLon * delta.x + cosLon * delta.y,
        -sinLat * cosLon * delta.x - sinLat * sinLon * delta.y + cosLat * delta.z,
        cosLat * cosLon * delta.x + cosLat * sinLon * delta.y + sinLat * delta.z
    };
}